#include "kinetic.h"
using namespace Ogre;

void Kinetic::update(Real time, Real engine_acceleration)
{
	Vector3 vn = v.normalisedCopy();
	Real area = AREA_BASE + AREA_RATE * vn.crossProduct(Ogre::Vector3(0,0,-1)).squaredLength();
	
	v -= area * time * (AIR_B * vn * v.squaredLength() + AIR_C * v);


	
	Real lifter = (AIR_D * (v.z * v.z) - AIR_E * (v.z));
	if (lifter > LIFT_THRESHOLD)
		lifter = LIFT_THRESHOLD + Ogre::Math::ATan(lifter - LIFT_THRESHOLD).valueDegrees() / 90
				 * (LIFT_MAX - LIFT_THRESHOLD);
	v.y += time * lifter;

	

	v.z -= engine_acceleration * time;
}


Vector3 Kinetic::get_velocity()
{
	return v;
}

void Kinetic::affect(SceneNode* node, Real time, Real a)
{
	update(time, a);

	w *= Math::Exp(-ANG_RATE * time);

	// rotate the object and update the velocity (was rotated relatively)

	Vector3 tx, ty, tz, v2;
	Quaternion ort = node->getOrientation();
	ort.ToAxes(tx, ty, tz);

	v2 = v.x * tx + v.y * ty + v.z * tz;
	node->translate(v2 * time * 5);

	node->rotate(Quaternion(Radian(w.length()), w.normalisedCopy()), Node::TS_LOCAL);
	
	node->getOrientation().ToAxes(tx, ty, tz);
	
	v.x = 0.7f * v.x + 0.3f * v2.dotProduct(tx);
	v.y = 0.7f * v.y + 0.3f * v2.dotProduct(ty);
	v.z = 0.7f * v.z + 0.3f * v2.dotProduct(tz);
	
}

Kinetic::Kinetic(): v(Vector3::ZERO), w(Vector3::ZERO) {}

void Kinetic::setup(Vector3 vt)
{
	v = vt;
	w = Vector3::ZERO;
}

void Kinetic::force_work(Vector3 f, Ogre::Real t)
{
	v += f * t;
}

void Kinetic::torque_work(Vector3 q, Real t)
{
	w += q * t;
}

void Kinetic::roll_work(Real rate, Real t)
{
	torque_work(Vector3(ROT_RATE * 0.3f * rate, 0, 0), t);
}

void Kinetic::pitch_work(Real rate, Real t)
{
	torque_work(Vector3(0, 0, ROT_RATE * rate), t);
	//v.z -= ACCELERATE_RATE * 0.3f * rate * rate * t;
}

void Kinetic::yaw_work(Real rate, Real t)
{
	torque_work(Vector3(0, ROT_RATE * 0.2f * rate, 0), t);
}